#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "controller_interface.h"
#include <quad_common/carmen3d_2d_hacks.h>
#include <bot_core/math_util.h>

#define LAND_GOAL -1.0

static inline void _copy_waypt_xyzt(quad_waypoint_t * dest,
                                    const quad_waypoint_t * src) {
  int i;
  for (i = 0; i < 4; i++)
    dest->xyzt[i] = src->xyzt[i];
}
static inline void _zero_waypt_vel(quad_waypoint_t * waypt) {
  int i;
  for (i = 0; i < 4; i++)
    waypt->xyzt_dot[i] = 0;
}

int carmen3d_controller_update_waypoints(waypoint_params_t * waypt_params,
                                         lcm_t * lcm,
                                         const quad_waypoint_t *new_waypt,
                                         quad_waypoint_t * start_point,
                                         quad_waypoint_t *current_set_point,
                                         quad_waypoint_t *goal,
                                         quad_waypoint_t * current_pose,
                                         int goalReached, int64_t current_utime)

{
  if (goal->waypt_type == QUAD_WAYPOINT_T_TYPE_USER_STOP
      && new_waypt->utime - goal->utime < 2e6) {
    fprintf(stderr, "waypoint came too soon after stop... ignoring it!\n");
    quad_waypoint_t_publish(lcm, WAYPOINT_IGNORED_CHANNEL, new_waypt);  //publish on ignored channel for logging
    return 0;  //ignore waypoints that arrive within 2 seconds of a stop waypoint
  }

  if (goal->waypt_type == QUAD_WAYPOINT_T_TYPE_LAND
      && new_waypt->waypt_type != QUAD_WAYPOINT_T_TYPE_TAKEOFF) {
    fprintf(stderr,
            "only takeoff is valid from LAND state... ignoring waypt!\n");
    quad_waypoint_t_publish(lcm, WAYPOINT_IGNORED_CHANNEL, new_waypt);  //publish on ignored channel for logging
    return 0;
  }

  if (goal->waypt_type == QUAD_WAYPOINT_T_TYPE_TAKEOFF
      && new_waypt->utime - goal->utime < 2e6) {
    fprintf(stderr, "waypoint came too soon after takeoff... ignoring it!\n");
    quad_waypoint_t_publish(lcm, WAYPOINT_IGNORED_CHANNEL, new_waypt);  //publish on ignored channel for logging
    return 0;  //ignore waypoints that arrive within 2 seconds of a takeoff waypoint
  }

  if (goal->waypt_type == QUAD_WAYPOINT_T_TYPE_LAND
      && new_waypt->utime - goal->utime < 2e6) {
    fprintf(stderr, "waypoint came too soon after land... ignoring it!\n");
    quad_waypoint_t_publish(lcm, WAYPOINT_IGNORED_CHANNEL, new_waypt);  //publish on ignored channel for logging
    return 0;  //ignore waypoints that arrive within 2 seconds of a land waypoint
  }

  //  if (goal->utime > new_waypt->utime) {
  //    fprintf(stderr, "waypoint has timestamp before the last one... ignoring it!\n");
  //    quad_waypoint_t_publish(lcm, WAYPOINT_IGNORED_CHANNEL, new_waypt); //publish on ignored channel for logging
  //    return 0; //ignore waypoints that arrive within 2 seconds of a land waypoint
  //  }

  if (goal->waypt_type == QUAD_WAYPOINT_T_TYPE_WAYPT
      && new_waypt->nonce == goal->nonce) {
    fprintf(stderr, "waypoint has same nonce as prev... ignoring it!\n");
    quad_waypoint_t_publish(lcm, WAYPOINT_IGNORED_CHANNEL, new_waypt);  //publish on ignored channel for logging
    return 0;
  }

  if (new_waypt->nonce < 0) {
    fprintf(stderr, "waypoint nonce is negative!... ignoring it!\n");
    quad_waypoint_t_publish(lcm, WAYPOINT_IGNORED_CHANNEL, new_waypt);  //publish on ignored channel for logging
    return 0;
  }

  if (new_waypt->xyzt[0] == QUAD_WAYPOINT_T_USE_DEFAULT) {
    fprintf(stderr, "ERRORreceived invalid waypt!, X cannot be default!\n");
    quad_waypoint_t_publish(lcm, WAYPOINT_IGNORED_CHANNEL, new_waypt);  //publish on ignored channel for logging
    return 0;
  }
  if (new_waypt->xyzt[1] == QUAD_WAYPOINT_T_USE_DEFAULT) {
    fprintf(stderr, "ERRORreceived invalid waypt!, Y cannot be default!\n");
    quad_waypoint_t_publish(lcm, WAYPOINT_IGNORED_CHANNEL, new_waypt);  //publish on ignored channel for logging
    return 0;
  }

  if (new_waypt->xyzt[3] != QUAD_WAYPOINT_T_USE_DEFAULT
      && new_waypt->xyzt[3] != QUAD_WAYPOINT_T_USE_PREVIOUS
      && (new_waypt->xyzt[3] < -M_PI || new_waypt->xyzt[3] > M_PI)) {
    fprintf(
        stderr,
        "ERROR received invalid waypt!, Theta must be between -PI and PI!!\n");
    quad_waypoint_t_publish(lcm, WAYPOINT_IGNORED_CHANNEL, new_waypt);  //publish on ignored channel for logging
    return 0;
  }

  static int64_t lastBodyRelativeTime = -1;
  int64_t bodyRelativePrintInterval = 1e6;

  double clampDist = .1;  //at 10hz, this means that obstacle avoidance will move us at most 1m/s in x and y

  switch (new_waypt->waypt_type) {
    case QUAD_WAYPOINT_T_TYPE_WAYPT: {
      if (waypt_params->enforceGoalReached && !goalReached) {
        fprintf(stderr, "Goal Not reached, ignoring WAYPT waypoint!\n");
        quad_waypoint_t_publish(lcm, WAYPOINT_IGNORED_CHANNEL, new_waypt);  //publish on ignored channel for logging
        return 0;
      }
      //set the previous desired pose to the current quad hover point so that we go from wherever we are...
      _copy_waypt_xyzt(start_point, current_set_point);

      //fill in the desired positions with the appropriate values
      if (new_waypt->xyzt[0] != QUAD_WAYPOINT_T_USE_PREVIOUS)
        goal->xyzt[0] = new_waypt->xyzt[0];

      if (new_waypt->xyzt[1] != QUAD_WAYPOINT_T_USE_PREVIOUS)
        goal->xyzt[1] = new_waypt->xyzt[1];

      if (new_waypt->xyzt[2] == QUAD_WAYPOINT_T_USE_DEFAULT)
        goal->xyzt[2] = waypt_params->default_height;
      else if (new_waypt->xyzt[2] != QUAD_WAYPOINT_T_USE_PREVIOUS)
        goal->xyzt[2] = new_waypt->xyzt[2];

      if (new_waypt->xyzt[3] == QUAD_WAYPOINT_T_USE_DEFAULT)
        goal->xyzt[3] = waypt_params->default_yaw;
      else if (new_waypt->xyzt[3] != QUAD_WAYPOINT_T_USE_PREVIOUS)
        goal->xyzt[3] = new_waypt->xyzt[3];

      //fill in velocities
      if (new_waypt->xyzt_dot[0]
          <= 0|| new_waypt->xyzt_dot[0] == QUAD_WAYPOINT_T_USE_DEFAULT)goal->xyzt_dot[0] = waypt_params->default_xy_vel;
      else if (new_waypt->xyzt_dot[0] != QUAD_WAYPOINT_T_USE_PREVIOUS)
      goal->xyzt_dot[0] = new_waypt->xyzt_dot[0];

      if (new_waypt->xyzt_dot[1]
          <= 0|| new_waypt->xyzt_dot[1] == QUAD_WAYPOINT_T_USE_DEFAULT)goal->xyzt_dot[1] = waypt_params->default_xy_vel;
      else if (new_waypt->xyzt_dot[1] != QUAD_WAYPOINT_T_USE_PREVIOUS)
      goal->xyzt_dot[1] = new_waypt->xyzt_dot[1];

      if (new_waypt->xyzt_dot[2]
          <= 0|| new_waypt->xyzt_dot[2] == QUAD_WAYPOINT_T_USE_DEFAULT)goal->xyzt_dot[2] = waypt_params->default_z_vel;
      else if (new_waypt->xyzt_dot[2] != QUAD_WAYPOINT_T_USE_PREVIOUS)
      goal->xyzt_dot[2] = new_waypt->xyzt_dot[2];

      if (new_waypt->xyzt_dot[3]
          <= 0|| new_waypt->xyzt_dot[3] == QUAD_WAYPOINT_T_USE_DEFAULT)goal->xyzt_dot[3] = waypt_params->default_yaw_vel;
      else if (new_waypt->xyzt_dot[3] != QUAD_WAYPOINT_T_USE_PREVIOUS)
      goal->xyzt_dot[3] = new_waypt->xyzt_dot[3];

      //set the timestamp for starting to head toward this goal
      goal->utime = current_utime;
      goal->waypt_type = new_waypt->waypt_type;
      //      update current set_point
      _zero_waypt_vel(current_set_point);  //zero out velocities
      current_set_point->utime = current_utime;

      fprintf(stderr, "WAYPT waypoint: ");
      break;
    }
    case QUAD_WAYPOINT_T_TYPE_LAND: {
      //set the previous desired pose to the current quad position so that we go from wherever we are...
      _copy_waypt_xyzt(start_point, current_set_point);
      _copy_waypt_xyzt(goal, current_set_point);
      goal->xyzt[2] = LAND_GOAL;
      goal->xyzt_dot[2] = waypt_params->default_land_vel;
      goal->xyzt_dot[0] = 0;
      goal->xyzt_dot[1] = 0;
      //set the timestamp for starting to head toward this goal
      goal->utime = current_utime;
      goal->waypt_type = new_waypt->waypt_type;
      //      update current set_point
      _zero_waypt_vel(current_set_point);  //zero out velocities
      current_set_point->utime = current_utime;
      current_set_point->waypt_type = QUAD_WAYPOINT_T_TYPE_LAND;
      goal->waypt_type = QUAD_WAYPOINT_T_TYPE_LAND;

      fprintf(stderr, "LAND waypoint: ");
      break;
    }
    case QUAD_WAYPOINT_T_TYPE_TAKEOFF: {
      _copy_waypt_xyzt(start_point, current_pose);  //we want to take off from wherever we are...
      _copy_waypt_xyzt(current_set_point, current_pose);
      _copy_waypt_xyzt(goal, current_pose);  //want to take off to right above us
      goal->xyzt[2] = waypt_params->default_takeoff_height;
      goal->xyzt_dot[2] = waypt_params->default_takeoff_vel;
      goal->xyzt_dot[0] = 0;
      goal->xyzt_dot[1] = 0;
      //set the timestamp for starting to head toward this goal
      goal->utime = current_utime;
      goal->waypt_type = new_waypt->waypt_type;
      //      update current set_point
      _zero_waypt_vel(current_set_point);  //zero out velocities
      current_set_point->utime = current_utime;
      current_set_point->waypt_type = QUAD_WAYPOINT_T_TYPE_TAKEOFF;
      goal->waypt_type = QUAD_WAYPOINT_T_TYPE_TAKEOFF;

      fprintf(stderr, "TAKEOFF waypoint: ");
      break;
    }
    case QUAD_WAYPOINT_T_TYPE_STOP:
    case QUAD_WAYPOINT_T_TYPE_USER_STOP: {
      //we want to stop wherever we are currently hovering around...
      _copy_waypt_xyzt(start_point, current_set_point);
      _copy_waypt_xyzt(goal, current_set_point);
      goal->xyzt_dot[0] = 0;
      goal->xyzt_dot[1] = 0;
      goal->xyzt_dot[2] = 0;
      //set the timestamp for starting to head toward this goal
      goal->utime = current_utime;
      goal->waypt_type = new_waypt->waypt_type;
      //      update current set_point
      _zero_waypt_vel(current_set_point);  //zero out velocities
      current_set_point->utime = current_utime;

      fprintf(stderr, "STOP waypoint: ");
      break;
    }
    case QUAD_WAYPOINT_T_TYPE_RELATIVE: {
      //update the desired_pose and prev_desired_pose... leave everything else the same
      if (new_waypt->xyzt[0] != QUAD_WAYPOINT_T_USE_DEFAULT
          && new_waypt->xyzt[0] != QUAD_WAYPOINT_T_USE_PREVIOUS
          && fabs(new_waypt->xyzt[0]) < .25) {
        start_point->xyzt[0] += new_waypt->xyzt[0];
        current_set_point->xyzt[0] += new_waypt->xyzt[0];
        goal->xyzt[0] += new_waypt->xyzt[0];
      }
      if (new_waypt->xyzt[1] != QUAD_WAYPOINT_T_USE_DEFAULT
          && new_waypt->xyzt[1] != QUAD_WAYPOINT_T_USE_PREVIOUS
          && fabs(new_waypt->xyzt[1]) < .25) {
        start_point->xyzt[1] += new_waypt->xyzt[1];
        current_set_point->xyzt[1] += new_waypt->xyzt[1];
        goal->xyzt[1] += new_waypt->xyzt[1];
      }
      if (new_waypt->xyzt[2] != QUAD_WAYPOINT_T_USE_DEFAULT
          && new_waypt->xyzt[2] != QUAD_WAYPOINT_T_USE_PREVIOUS
          && fabs(new_waypt->xyzt[2]) < .15) {
        start_point->xyzt[2] += new_waypt->xyzt[2];
        current_set_point->xyzt[2] += new_waypt->xyzt[2];
        goal->xyzt[2] += new_waypt->xyzt[2];
      }
      if (new_waypt->xyzt[3] != QUAD_WAYPOINT_T_USE_DEFAULT
          && new_waypt->xyzt[3] != QUAD_WAYPOINT_T_USE_PREVIOUS
          && fabs(new_waypt->xyzt[3]) < .15) {
        start_point->xyzt[3] += new_waypt->xyzt[3];
        current_set_point->xyzt[3] += new_waypt->xyzt[3];
        goal->xyzt[3] += new_waypt->xyzt[3];
      }
      fprintf(stderr, "RELATIVE waypoint: ");

      break;
    }
    case QUAD_WAYPOINT_T_TYPE_BODY_RELATIVE: {
      quad_waypoint_t _body_waypt, _global_waypt;
      _copy_waypt_xyzt(&_body_waypt, new_waypt);
      if (_body_waypt.xyzt[0] == QUAD_WAYPOINT_T_USE_DEFAULT
          || _body_waypt.xyzt[0] == QUAD_WAYPOINT_T_USE_PREVIOUS) {
        _body_waypt.xyzt[0] = 0;
      }
      if (_body_waypt.xyzt[1] == QUAD_WAYPOINT_T_USE_DEFAULT
          || _body_waypt.xyzt[1] == QUAD_WAYPOINT_T_USE_PREVIOUS) {
        _body_waypt.xyzt[1] = 0;
      }

      //clamp values so we don't jump too far in one go
      _body_waypt.xyzt[0] =
          bot_clamp(_body_waypt.xyzt[0], -clampDist, clampDist);
      _body_waypt.xyzt[1] =
          bot_clamp(_body_waypt.xyzt[1], -clampDist, clampDist);
      //compute the new "global waypoint based on the current body position
      carmen3d_2d_hacks_vec_body_to_global(_body_waypt.xyzt,
                                           current_pose->xyzt[3],
                                           _global_waypt.xyzt);

      //move both the prev and desired pose, so that it has a direct effect, and doesn't get interpolated

      start_point->xyzt[0] += _global_waypt.xyzt[0];
      current_set_point->xyzt[0] += _global_waypt.xyzt[0];
      goal->xyzt[0] += _global_waypt.xyzt[0];

      start_point->xyzt[1] += _global_waypt.xyzt[1];
      current_set_point->xyzt[1] += _global_waypt.xyzt[1];
      goal->xyzt[1] += _global_waypt.xyzt[1];

      if ((new_waypt->utime - lastBodyRelativeTime)
          > bodyRelativePrintInterval) {
        fprintf(stderr, "BODY RELATIVE waypoint: ");
      }
      break;
    }
    default: {
      fprintf(stderr, "ERROR! RECEIVED UNKNOWN WAYPOINT TYPE from %d!!",
              new_waypt->sender);
      return 0;
    }
  }
  if (new_waypt->waypt_type != QUAD_WAYPOINT_T_TYPE_BODY_RELATIVE
      || (new_waypt->utime - lastBodyRelativeTime)
          > bodyRelativePrintInterval) {
    fprintf(
        stderr,
        "prev_desired_poset: (%f,%f,%f) %f \t desired_pose: (%f,%f,%f) %f\n",
        start_point->xyzt[0], start_point->xyzt[1], start_point->xyzt[2],
        start_point->xyzt[3], goal->xyzt[0], goal->xyzt[1], goal->xyzt[2],
        goal->xyzt[3]);
    lastBodyRelativeTime = current_utime;
  }
  goal->nonce = new_waypt->nonce;
  quad_waypoint_t_publish(lcm, WAYPOINT_GOAL_CHANNEL, goal);  //send out the current goal for plotting and display :-)
  return 1;
}

